/***************************************************************************\
 * Copyright (C) by Keio University
 * CVToGLMatrixConversion.cpp created in 10 2011.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * CVToGLMatrixConversion.cpp is part of the HVRL Engine Project.
 *
 * The HVRL Engine Project is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Project is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/tools/CVToGLMatrixConversion.hpp"
#include "hvrl/common/Common.hpp"

#include <glm/gtc/type_ptr.hpp>

namespace hvrl
{

void CVToGLConversion::extrinsicMatrix(const cv::Matx33f& R, cv::Vec3f& t,
		glm::mat4& glmodelview)
{

	glmodelview = glm::mat4();
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			glmodelview[j][i] = R(i, j);
		}
		glmodelview[3][i] = t[i];
	}

	glm::mat4 convert(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f,
			0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
	glmodelview = convert * glmodelview;

}

void CVToGLConversion::intrinsicMatrix(const cv::Matx33f& K, cv::Size windowsize,
		const float& n, const float& f, glm::mat4& glprojection)
{
	glprojection = glm::mat4();
	glprojection[0][0] = 2.0f * K(0, 0)
			/ static_cast<float> (windowsize.width);
	glprojection[1][1] = 2.0f * K(1, 1)
			/ static_cast<float> (windowsize.height);
	glprojection[2][0] = 1.0f - 2.0f * K(0, 2)
			/ static_cast<float> (windowsize.width);
	glprojection[2][1] = -1.0f + 2.0f * K(1, 2)
			/ static_cast<float> (windowsize.height);
	glprojection[2][2] = (n + f) / (n - f);
	glprojection[2][3] = -1.0f;
	glprojection[3][2] = (2.0f * n * f) / (n - f);
	glprojection[3][3] = 0.0f;
}

/***********************/
// double
/***********************/

void CVToGLConversion::extrinsicMatrix(const cv::Matx33d& R, cv::Vec3d& t,
		glm::dmat4& glmodelview)
{

	glmodelview = glm::dmat4();
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			glmodelview[j][i] = R(i, j);
		}
		glmodelview[3][i] = t[i];
	}

	glm::dmat4 convert(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f,
			0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
	glmodelview = convert * glmodelview;
}

void CVToGLConversion::intrinsicMatrix(const cv::Matx33d& K, cv::Size windowsize,
		const double& n, const double& f, glm::dmat4& glprojection)
{
	glprojection = glm::dmat4(0);
	glprojection[0][0] = 2.0 * K(0, 0)
			/ static_cast<double> (windowsize.width);
	glprojection[1][1] = 2.0 * K(1, 1)
			/ static_cast<double> (windowsize.height);
	glprojection[2][0] = 1.0 - 2.0 * K(0, 2)
			/ static_cast<double> (windowsize.width);
	glprojection[2][1] = -1.0 + 2.0 * K(1, 2)
			/ static_cast<double> (windowsize.height);
	glprojection[2][2] = (n + f) / (n - f);
	glprojection[2][3] = -1.0;
	glprojection[3][2] = (2.0 * n * f) / (n - f);
}

void CVToGLConversion::extrinsicMatrix(const cv::Matx33d& R, cv::Vec3d& t,
		double(&glmodelview)[16])
{
	glm::dmat4 m = glm::make_mat4(glmodelview);
	extrinsicMatrix(R, t, m);
	memcpy(glmodelview, glm::value_ptr(m), 16 * sizeof(double));
}

void CVToGLConversion::intrinsicMatrix(const cv::Matx33d& K, cv::Size windowsize,
		const double& near_, const double& far_, double(&glprojection)[16])
{
	glm::dmat4 m = glm::make_mat4(glprojection);
	intrinsicMatrix(K, windowsize, near_, far_, m);
	memcpy(glprojection, glm::value_ptr(m), 16 * sizeof(double));
}

void CVToGLConversion::extrinsicMatrix(const cv::Matx33f& R, cv::Vec3f& t,
		float(&glmodelview)[16])
{
	glm::mat4 m = glm::make_mat4(glmodelview);
	extrinsicMatrix(R, t, m);
	memcpy(glmodelview, glm::value_ptr(m), 16 * sizeof(float));
}

void CVToGLConversion::intrinsicMatrix(const cv::Matx33f& K, cv::Size windowsize,
		const float& near_, const float& far_, float(&glprojection)[16])
{
	glm::mat4 m = glm::make_mat4(glprojection);
	intrinsicMatrix(K, windowsize, near_, far_, m);
	memcpy(glprojection, glm::value_ptr(m), 16 * sizeof(float));
}

}
